/********** Local Includes ****************************************************/

#include "objectdetector.h"

#include "common.h"
#include "imagesource.h"
#include "robot_orientation.h"

/********** Macros and Constants **********************************************/

#define BLUR_SIZE 5
#define SEGMENT_THRESHOLD 50

#define DIFF_THRESHOLD 100

#define RED CV_RGB(255, 0, 0)
#define GREEN CV_RGB(0, 255, 0)
#define BLUE CV_RGB(0, 0, 255)

/********** Namespaces ********************************************************/

using namespace cv;
using namespace std;

/********** Local Function Prototypes *****************************************/

/**
 * Returns a matrix of uchars with 1 indicating that the pixel changed and 0
 * indicating that the pixel did not change.
 */
Mat diff(const Mat &first, const Mat &second);

int diff(const Vec3b &first, const Vec3b &second);

/**
 * Returns the Manhatten distance between the point and the (row, col)
 * coordinate.
 */
int manhatten(Point p, int row, int col);

Mat diffToImage(const Mat &diff);

Mat bufferObstacles(const Mat &diff, int bufferSize);

/********** Public Methods ****************************************************/

ObjectDetector::ObjectDetector(ImageSource *source) :
	m_source(source)
{
	if (!m_source) {
		ERROR("Image source cannot be null");
	}
}
	
bool ObjectDetector::saveBackground() {
	if (!m_source) {
		ERROR("Image source cannot be null");
		return false;
	}
	
	m_background = getAndBlur();
	return true;
}

bool ObjectDetector::saveObstacles() {
	if (!m_source) {
		ERROR("Image source cannot be null");
		return false;
	}
	
	m_obstacles = getAndBlur();
	Mat temp = diff(m_background, m_obstacles);
	m_open = diffToImage(temp);
	
	return true;
}

bool ObjectDetector::saveRobotAndFruit(Point robot, Point fruit, bool debug) {
	if (!m_source) {
		ERROR("Image source cannot be null");
		return false;
	}
	
	m_robotAndFruit = getAndBlur();
	m_rfDiff = diff(m_obstacles, m_robotAndFruit);
	
	// Remove the robot from the frame for easier processing later
	Size size = m_rfDiff.size();
	m_fruit = m_robotAndFruit.clone();
	
	for (int row = 0; row < size.height; row++) {
		for (int col = 0; col < size.width; col++) {
			if (m_rfDiff.at<uchar>(row, col)
				&& manhatten(robot, row, col) < manhatten(fruit, row, col))
			{
				m_fruit.at<Vec3b>(row, col) = m_obstacles.at<Vec3b>(row, col);
			}
		}
	}
	
	locateFruit();
	RobotOrientation robotCandidate = findRobot(m_robotAndFruit, debug);
	
	if (robotCandidate.point.x > 0 && robotCandidate.point.y > 0) {
		m_robotLast = robotCandidate;
	}
	
    Mat differences = diff(m_background, m_obstacles);
    m_open = bufferObstacles(differences, m_robotRadius);
	
	return true;
}

RobotOrientation ObjectDetector::findRobot(bool debug) {
	//m_robotAndFruit = getAndBlur();
	//return findRobot(m_robotAndFruit, debug);
	ERROR("This should never be called");
	return RobotOrientation();
}

RobotOrientations ObjectDetector::findRobots() {
	saveRobotAndFruit(robot().point, fruit(), false);
	
	RobotOrientations r;
	r.rovio = robot();
	r.pleo = fruit();
	
	return r;
}

Mat & ObjectDetector::getOpen() {
	return m_open;
}
	
Mat ObjectDetector::processed() {
	if (m_open.size().width == 0) {
		return m_background;
	}
	
	Mat temp;
	
	if (m_robotAndFruit.size().width == 0) {
		temp = m_obstacles.clone();
	} else {
		temp = m_robotAndFruit.clone();
	}
	
	Size size = temp.size();
    
	
	for (int row = 0; row < size.height; row++) {
		for (int col = 0; col < size.width; col++) {
			if (m_open.at<float>(row, col) == 0) {
				int i = temp.at<Vec3b>(row, col)[2];
				i += 100;
				
				if (i > 255) {
					i = 255;
				}
				
				temp.at<Vec3b>(row, col)[2] = i;
			}
		}
	}
	
	if (m_robotAndFruit.size().width == 0) {
		return temp;
	}
	
	Mat ofDiff = diff(m_obstacles, m_fruit);
	
	for (int row = 0; row < size.height; row++) {
		for (int col = 0; col < size.width; col++) {
			if (ofDiff.at<uchar>(row, col)) {
				int i = temp.at<Vec3b>(row, col)[1];
				i += 100;
				
				if (i > 255) {
					i = 255;
				}
				
				temp.at<Vec3b>(row, col)[1] = i;
			}
		}
	}
	
	Mat frDiff = diff(m_fruit, m_robotAndFruit);
	
	for (int row = 0; row < size.height; row++) {
		for (int col = 0; col < size.width; col++) {
			if (frDiff.at<uchar>(row, col)) {
				int i = temp.at<Vec3b>(row, col)[0];
				i += 100;
				
				if (i > 255) {
					i = 255;
				}
				
				temp.at<Vec3b>(row, col)[0] = i;
			}
		}
	}
	
	for (vector<Point>::const_iterator it = m_breadcrumbs.begin();
		it != m_breadcrumbs.end(); it++)
	{
		circle(temp, *it, 2, BLUE, 3);
	}
    
    circle(temp, m_breadcrumbs[m_breadcrumbs.size()-1], int(robotRadius()), BLUE, 3);
    circle(temp, m_fruitLast, 10, GREEN, 3);
	
	return temp;
}

RobotOrientation ObjectDetector::robot() const {
	return m_robotLast;
}

double ObjectDetector::robotRadius() const {
	return m_robotRadius;
}

Point ObjectDetector::fruit() const {
	return m_fruitLast;
}

/********** Private Methods ***************************************************/

Mat ObjectDetector::getAndBlur() const {
	Mat next = m_source->next();
	blur(next, next, Size(BLUR_SIZE, BLUR_SIZE));
	return next;
}

RobotOrientation ObjectDetector::findRobot(const Mat &frame, bool debug) {
	Mat robot;
	Mat fDiff = diff(m_fruit, frame);
	frame.copyTo(robot, fDiff);
	
	map<BlobDetector::BlobCategory, Point> points = m_bd.getPoints(robot);
	
	Point left = points[BlobDetector::ROBOT_LEFT];
	Point right = points[BlobDetector::ROBOT_RIGHT];
	
	if (debug) {
		Mat temp = frame.clone();
		circle(temp, left, 20, BLUE, 3);
		circle(temp, right, 20, GREEN, 3);
		
		imshow("Robot Blobs", temp);
	}
	
	m_robotRadius = euclidean(left, right) * 1.75;
	RobotOrientation ro = getRobotOrientation(left, right);
	
	m_breadcrumbs.push_back(ro.point);
	return ro;
}

void ObjectDetector::locateFruit() {
	Mat d = diff(m_obstacles, m_fruit);
	
	long rowSum = 0;
	long colSum = 0;
	int count = 0;
	
	for (int row = 0; row < d.size().height; row++) {
		for (int col = 0; col < d.size().width; col++) {
			if (d.at<uchar>(row, col) == 1) {
				rowSum += row;
				colSum += col;
				count++;
			}
		}
	}
	
	Point fruitCandidate = Point(int(colSum / count), int(rowSum / count));
	
	if (fruitCandidate.x > 0 && fruitCandidate.y > 0) {
		m_fruitLast = fruitCandidate;
	}
}

double ObjectDetector::euclidean(Point one, Point two) const {
	double dx = two.x - one.x;
	double dy = two.y - one.y;
	
	return sqrt(dx * dx + dy * dy);
}

/********** Local Functions ***************************************************/

Mat diff(const Mat &first, const Mat &second) {
	if (first.size() != second.size()) {
		ERROR("Image sizes must be equal!");
		DUMP(first.size().width);
		DUMP(first.size().height);
		DUMP(second.size().width);
		DUMP(second.size().height);
		return Mat();
	}
	
	Size size = first.size();
	Mat result(size, CV_8UC1);
	
	for (int row = 0; row < size.height; row++) {
		for (int col = 0; col < size.width; col++) {
			int d = diff(first.at<Vec3b>(row, col), second.at<Vec3b>(row, col));
			
			if (d > DIFF_THRESHOLD) {
				result.at<uchar>(row, col) = 1;
			} else {
				result.at<uchar>(row, col) = 0;
			}
		}
	}
	
	return result;
}

int diff(const Vec3b &first, const Vec3b &second) {
	int sum = 0;
	
	for (int i = 0; i < 3; i++) {
		sum += abs(first[i] - second[i]);
	}
	
	return sum;
}

int manhatten(Point p, int row, int col) {
	return abs(p.x - col) + abs(p.y - row);
}

//1 is clear
//0 is block
Mat diffToImage(const Mat &diff) {
	Mat temp = Mat(diff.size(), CV_32FC1);
	Size size = diff.size();
   
    
	for (int row = 0; row < size.height; row++) {
		for (int col = 0; col < size.width; col++) {
			temp.at<float>(row, col) = 1 - diff.at<uchar>(row, col);
            
		}
	}
    
	return temp;
}

Mat bufferObstacles(const Mat &diff, int bufferSize) {
	Mat temp = Mat(diff.size(), CV_32FC1);
	Size size = diff.size();
    
    
	for (int row = 0; row < size.height; row++) {
		for (int col = 0; col < size.width; col++) {
			temp.at<float>(row, col) = 1 - diff.at<uchar>(row, col);
            
		}
	}
    
    Mat buffered = temp.clone();
    for (int row = 0; row < size.height; row++) {
		for (int col = 0; col < size.width; col++) {
            
            if (temp.at<float>(row, col) <.1) {//blocked
                //draw buffer
                Point start(col - bufferSize, row - bufferSize);
                Point end(col + bufferSize, row+bufferSize);
                rectangle(buffered, start, end, 0,-1);
                
            }
		}
	}
    
	return buffered;
}